package com.gitee.wsl.mathematics.matrix.mat4.ext

import com.gitee.wsl.mathematics.complex.quaternion.Quaternion
import com.gitee.wsl.mathematics.matrix.mat4.MutableMatrix4
import com.gitee.wsl.mathematics.vector.vec3.Vector3


/**
 * Applies the composition of the given [translation], [rotation] and [scale] to this transform matrix.
 */
fun<T:Number> MutableMatrix4<T, *, *>.compose(translation: Vector3<T, *>, rotation: Quaternion<T, *>, scale: Vector3<T, *> = createVec3(one,one,one)) {
    val x = rotation.x; val y = rotation.y; val z = rotation.z; val w = rotation.w
    val x2 = x + x;  val y2 = y + y;  val z2 = z + z
    val xx = x * x2; val xy = x * y2; val xz = x * z2
    val yy = y * y2; val yz = y * z2; val zz = z * z2
    val wx = w * x2; val wy = w * y2; val wz = w * z2

    val sx = scale.x; val sy = scale.y; val sz = scale.z

    mul(
        (1 - (yy + zz)) * sx,   (xy - wz) * sy,         (xz + wy) * sz,         translation.x,
        (xy + wz) * sx,         (1 - (xx + zz)) * sy,   (yz - wx) * sz,         translation.y,
        (xz - wy) * sx,         (yz + wx) * sy,         (1 - (xx + yy)) * sz,   translation.z,
        zero,                            zero,                     zero,                     one
    )
}

/**
 * Decomposes this transform matrix into its translation, rotation and scale components and returns them
 * int the provided [translation], [rotation] and [scale] vectors.
 */
/*fun decompose(translation: MutableVec3d? = null, rotation: MutableQuatD? = null, scale: MutableVec3d? = null) {
    translation?.set(m03, m13, m23)

    var sx = sqrt(m00*m00 + m10*m10 + m20*m20)
    val sy = sqrt(m01*m01 + m11*m11 + m21*m21)
    val sz = sqrt(m02*m02 + m12*m12 + m22*m22)
    if (determinant() < 0.0) {
        sx *= -1.0
    }
    scale?.set(sx, sy, sz)

    if (rotation != null) {
        val r00 = m00 / sx; val r01 = m01 / sy; val r02 = m02 / sz
        val r10 = m10 / sx; val r11 = m11 / sy; val r12 = m12 / sz
        val r20 = m20 / sx; val r21 = m21 / sy; val r22 = m22 / sz

        val trace = r00 + r11 + r22
        if (trace > 0.0) {
            val s = 0.5 / sqrt(trace + 1.0)
            rotation.set((r21 - r12) * s, (r02 - r20) * s, (r10 - r01) * s, 0.25 / s)
        } else {
            if (r00 < r11) {
                if (r11 < r22) {
                    var s = 0.5 / sqrt(r22 - r00 - r11 + 1.0)
                    if (r10 < r01) s = -s   // ensure non-negative w
                    rotation.set((r02 + r20) * s, (r12 + r21) * s, 0.25 / s, (r10 - r01) * s)

                } else {
                    var s = 0.5 / sqrt(r11 - r22 - r00 + 1.0)
                    if (r02 < r20) s = -s   // ensure non-negative w
                    rotation.set((r01 + r10) * s, 0.25 / s, (r21 + r12) * s, (r02 - r20) * s)
                }
            } else {
                if (r00 < r22) {
                    var s = 0.5 / sqrt(r22 - r00 - r11 + 1.0)
                    if (r10 < r01) s = -s   // ensure non-negative w
                    rotation.set((r02 + r20) * s, (r12 + r21) * s, 0.25 / s, (r10 - r01) * s)
                } else {
                    var s = 0.5 / sqrt(r00 - r11 - r22 + 1.0)
                    if (r21 < r12) s = -s   // ensure non-negative w
                    rotation.set(0.25 / s, (r10 + r01) * s, (r20 + r02) * s, (r21 - r12) * s)
                }
            }
        }
        rotation.norm()
    }
}*/
